#include <array>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>

using namespace std;
// using namespace Eigen;
using arr4r = array<double, 4>;
using arr2r = array<double, 2>;
using arr3r = array<double, 3>;
using vecArr3r = vector<arr3r>;
using vecArr4r = vector<arr4r>;
using vecArr2r = vector<arr2r>;

#include <Eigen/Dense>
#include <iostream>
/// 返回　x^order
inline double polyVal(const double x, const int order) {
  double val{1.0};
  for (int i = 1; i <= order; ++i)
    val *= x;
  return val;
}

/**
 * @brief 测试多项式插值缩放的影响
 * 
 */

class PolyInterp {
public:
  int nrow_, ncol_, order_; ///< order为多项式最高次数
  const double singular_val_threshold_ = 1.0e-14;
  Eigen::MatrixXd vand_;
  Eigen::MatrixXd sing_inv_;

public:
  void init(int nrow, int poly_order) {
    nrow_ = nrow;
    order_ = poly_order;
    ncol_ = (order_ + 1) * (order_ + 2) / 2;
    vand_ = Eigen::MatrixXd::Zero(nrow_, ncol_);
    sing_inv_ = Eigen::MatrixXd::Zero(ncol_, nrow_);
  }
  /// 构造范德蒙德矩阵的行
  void constructVandRow(const arr2r &dxy, const int row_idx) {
    vand_(row_idx, 0) = 1.0;
    int cnt{1};
    for (int od = 1; od <= order_; ++od)
      for (int i = 0; i < od + 1; ++i)
        vand_(row_idx, cnt++) = polyVal(dxy[0], od - i) * polyVal(dxy[1], i);
    assert(cnt == ncol_);
  }
  /// 返回范德蒙德矩阵逆的第几行  v_size为vand矩阵的行数
  void vandInvRow(const int row_idx, const int v_size, double *val) {
    assert(v_size == nrow_);
    const auto &svd = vand_.bdcSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
    // std::cout << svd.rank() << std::endl;
    const auto &sval = svd.singularValues();
    const int size = sval.size();
    for (int i = 0; i < size; ++i) {
      sing_inv_(i, i) = 0.0;
      //只有满足一定大小条件的特征值才会取逆，否则设置为零
      if (sval[i] > singular_val_threshold_)
        sing_inv_(i, i) = 1.0 / sval[i];
    }
    Eigen::Map<Eigen::RowVectorXd> v(val, v_size);
    v = svd.matrixV().row(row_idx) * sing_inv_ * svd.matrixU().transpose();
    // std::cout << v << std::endl;
  }
};

struct Fun3 {
  double coef{1.0};
  double operator()(const arr2r &x) const {
    return (x[0] * x[0] * x[0] * x[0] * x[0] +
            x[1] * x[1] * x[1] * x[1] * x[1]) *
           coef;
    // return cos(x[0] * PI * 10) * sin(x[1] * PI * 10);
  }
};

int main(int argc, char *argv[]) {
  if (argc != 2) {
    std::cerr << "./a.out num" << std::endl;
    exit(EXIT_FAILURE);
  }
  const int num = stoi(argv[1]);
  double min_max[2]{0, 1};
  double dxdy[2]{1.0 / num, 2.0 / num};
  cout << setprecision(16) << scientific;

  Fun3 f;
  arr2r xy[6][6];
  // double val[6][6];
  Eigen::VectorXd val(36);
  int count{0};
  for (int i = 0; i < 6; ++i)
    for (int j = 0; j < 6; ++j) {
      xy[i][j][0] = i * dxdy[0];
      xy[i][j][1] = j * dxdy[1];
      val(count++) = f(xy[i][j]);
    }
  arr2r p{2.3 * dxdy[0], 3.3 * dxdy[1]};
  double rval = f(p);

  PolyInterp interp;
  interp.init(36, 5);
  double maxx{5 * dxdy[0]}, maxy{5 * dxdy[1]}, minx{0}, miny{0},
      centx{0.5 * (maxx + minx)}, centy{0.5 * (maxy + minx)};
  double rng = fmax(maxx - minx, maxy - miny);

  int cnt{0};
  for (int i = 0; i < 6; ++i)
    for (int j = 0; j < 6; ++j) {
      arr2r p{(xy[i][j][0] - centx) / rng, (xy[i][j][1] - centy) / rng};
      interp.constructVandRow(p, cnt);
      ++cnt;
    }
  Eigen::VectorXd coe = interp.vand_.colPivHouseholderQr().solve(val);
  cout << coe << endl;

  arr2r p2{(p[0] - centx) / rng, (p[1] - centy) / rng};
  Eigen::VectorXd pxy(interp.ncol_);
  pxy(0) = 1.0;
  int cnt2{1};
  for (int od = 1; od <= 5; ++od)
    for (int i = 0; i < od + 1; ++i)
      pxy(cnt2++) = polyVal(p2[0], od - i) * polyVal(p2[1], i);

  // interp.constructVandRow(p2, 0);
  cout << "dlflsf" << endl;
  double interp_val = pxy.dot(coe);

  cout << interp_val << "  " << rval << "  " << interp_val - rval << endl;
  return 0;
}
